// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>

// ros2 bag record /joint1_p /joint2_p /joint3_p /joint4_p /y_ros2msg /big_valve_state /end_action
// 3.自定义节点类；
class Relay_Node : public rclcpp::Node
{
public:
    Relay_Node() : Node("relay_node_node_cpp")
    {
        this->x_sub_ = this->create_subscription<std_msgs::msg::Float64>("x_goat", 10, std::bind(&Relay_Node::x_cb, this, std::placeholders::_1));
        this->y_sub_ = this->create_subscription<std_msgs::msg::Float64>("y_goat", 10, std::bind(&Relay_Node::y_cb, this, std::placeholders::_1));
        this->z_sub_ = this->create_subscription<std_msgs::msg::Float64>("z_goat", 10, std::bind(&Relay_Node::z_cb, this, std::placeholders::_1));
        tar_x = this->create_publisher<std_msgs::msg::Float64>("x_go", 10);
        tar_y = this->create_publisher<std_msgs::msg::Float64>("y_go", 10);
        tar_z = this->create_publisher<std_msgs::msg::Float64>("z_go", 10);
    }

private:
    // 逆解所需的x、y、z坐标，来自qt或者control程序
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr x_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr y_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr z_sub_;

    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_x;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_y;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_z;

    // 订阅并赋值x、y、z(逆解)；
    void x_cb(const std_msgs::msg::Float64::SharedPtr x_msg)
    {
        auto x = std_msgs::msg::Float64();
        x.data = x_msg->data;
        tar_x->publish(x);
        printf("x_msg = %f \n",x_msg->data);
    }
    void y_cb(const std_msgs::msg::Float64::SharedPtr y_msg)
    {
        auto y = std_msgs::msg::Float64();
        y.data = y_msg->data;
        tar_y->publish(y);
        printf("y_msg = %f \n",y_msg->data);

    }
    void z_cb(const std_msgs::msg::Float64::SharedPtr z_msg)
    {
        auto z = std_msgs::msg::Float64();
        z.data = z_msg->data;
        tar_z->publish(z);
        printf("z_msg = %f \n",z_msg->data);

    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<Relay_Node>());
    // 5.资源释放
    rclcpp::shutdown();
    return 0;
}





